import sensor
import image
import lcd
import KPU as kpu
from machine import Timer,PWM
import utime
import time
from Maix import GPIO
from fpioa_manager import fm


#PWM通过定时器配置，接到IO17引脚（Pin IO17）
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
tim1 = Timer(Timer.TIMER0, Timer.CHANNEL1, mode=Timer.MODE_PWM)
S1 = PWM(tim, freq=50, duty=0, pin=15)
S2 = PWM(tim1, freq=50, duty=0, pin=17)

up_servo_value = 50
down_servo_value = -24
PD = {'KP':0.19,'KD':0.1}
now_error = {'x':0,'y':0}
last_error = {'x':0,'y':0}

Timing_time = 0
distance = 0
start = time.ticks_us()

fm.register(11, fm.fpioa.GPIOHS0)
R = GPIO(GPIO.GPIOHS0, GPIO.IN, GPIO.PULL_UP)
fm.register(13, fm.fpioa.GPIOHS13)
K1 = GPIO(GPIO.GPIOHS13, GPIO.IN,GPIO.PULL_UP)
fm.register(14, fm.fpioa.GPIOHS14)
K2 = GPIO(GPIO.GPIOHS14, GPIO.IN,GPIO.PULL_UP)

fm.register(12, fm.fpioa.GPIO0)
C = GPIO(GPIO.GPIO0, GPIO.OUT)
C.value(0)
fm.register(16, fm.fpioa.GPIO1)#HC_SR04的Trig引脚
T = GPIO(GPIO.GPIO1,GPIO.OUT)
T.value(0)

lcd.init()
#lcd.mirror(True)
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
#sensor.set_hmirror(1)
sensor.run(1)
task = kpu.load(0x300000) # you need put model(face.kfpkg) in flash at address 0x300000
# task = kpu.load("/sd/face.kmodel")
anchor = (1.889, 2.5245, 2.9465, 3.94056, 3.99987, 5.3658, 5.155437, 6.92275, 6.718375, 9.01025)
a = kpu.init_yolo2(task, 0.5, 0.3, 5, anchor)

def Servo(servo,angle):
    servo.duty((angle+90)/180*10+2.5)

def HC_SR04 (pin_num):#外部中断回调函数
    global Timing_time,distance,start
    if R.value() == 1 and T.value() == 0:                  #判断是上升沿
        start = time.ticks_us()           #
    else:                                 #判断是下降沿
        Timing_time = time.ticks_diff(time.ticks_us(),start)
        distance = (Timing_time*0.034)/2   #计算距离CM
        C.value(1)

R.irq(HC_SR04,GPIO.IRQ_BOTH, GPIO.WAKEUP_NOT_SUPPORT,5)#外部引脚中断配置

while(True):
    img = sensor.snapshot()
    code = kpu.run_yolo2(task, img)
    if code:
        i = code[0]
        a = img.draw_rectangle(i.rect())
        cx = i.rect()
        x = cx[0]+cx[2]/2
        y = cx[1]+cx[3]/2
        now_error['x'] = (160-x)
        now_error['y'] = (120-y)
        up_servo_value -= (PD['KP']*now_error['y']+PD['KD']*(last_error['y']-now_error['y']))
        if up_servo_value >= 80 : up_servo_value = 80#做限幅（扯坏了两个摄像头的教训）
        if up_servo_value <= 20 : up_servo_value = 20
        Servo(S1,up_servo_value)    #上
        down_servo_value += (PD['KP']*now_error['x']+PD['KD']*(last_error['x']-now_error['x']))
        if down_servo_value >= 16 : down_servo_value = 16
        if down_servo_value <= -64 : down_servo_value = -64
        Servo(S2,down_servo_value)   #下

        if now_error['x']<5 and now_error['y']<5 and now_error['x']>-5 and now_error['y']>-5 and C.value()==0:
            T.value(1)
            time.sleep_us(10)
            T.value(0)
            time.sleep_ms(50)
        elif now_error['x']>5 and now_error['y']>5 and now_error['x']<-5 and now_error['y']<-5:
            C.value(0)
    else:
        C.value(0)
    a = img.draw_string(40, 120, "distance:%0.3f"%distance,color=(128,0,0), scale=1)
    a = img.draw_string(40, 170, "KP:%0.3f"%PD['KP'],color=(128,0,0), scale=1)
    a = img.draw_string(40, 180, "KD:%0.3f"%PD['KD'],color=(128,0,0), scale=1)
    a = img.draw_string(40, 210, "down_servo_value:%f"%down_servo_value,color=(128,0,0), scale=1)
    a = img.draw_string(40, 220, "up_servo_value:%f"%up_servo_value, color=(128,0,0),scale=1)
    a = lcd.display(img)

a = kpu.deinit(task)
